//Standalone Joystick 2.4ghz TX
//Hadley Boks-Wilson 27/08/2011
//ver 1.1
//Many thanks to Ian Johnston for the origonal code i started from
//http://www.ianjohnston.com
// Many thanks to Ian and Hadley, since this is where I started from - Chris
#include <EEPROM.h>

// The Variables
String type;

// The pins we need
// Up to 8 channels without a Button Matrix
int pinThrottle, pinRoll, pinPitch, pinYaw, pinAux1, pinAux2, pinAux3, pinAux4, pinAux5, pinAux6, pinAux7, pinAux8;
int pinTrimRollU, pinTrimRollD, pinTrimPitchU, pinTrimPitchD, pinTrimYawU, pinTrimYawD;
int pinPPM, pinBuzzer;

// the trimmers
int trimRollSave, trimPitchSave, trimYawSave;
int trimYaw, trimPitch, trimRoll;
int trimUpdate, buttonUpdate;
int minTrim, maxTrim;

// These are the raw values read from the joystick (0-1023)
int rawThrottle, rawRoll, rawPitch, rawYaw;

// These values hold the values after the trim was applied
int throttle, roll, pitch, yaw;

// PPM timings
int pulseYaw, pulsePitch, pulseThrottle, pulseRoll, pulseAux1;
int lowPPM, pulseMin, pulseMax, pulseDiff;

// other channels
boolean light, arm;

// Misc
int tick, tickButton;
int channels;
int aux1State;

// Interrupt Service Routine
// This generates the PPM Signal every xx ms
ISR(TIMER1_COMPA_vect) {
  ppmoutput();
}

// Setup is run only once on startup and initializes all global variables
void setup() {
  // assign the pins according to arduino type
  config();

  // We set the initial timing for PPM in us
  // TODO: Set this via config file
  lowPPM = 300;  // fixed LOW phase 
  pulseMin = 600;
  pulseMax = 1800;
  pulseDiff = pulseMax - pulseMin;
  
  pulseYaw = pulseMin;
  pulsePitch = pulseMin;
  pulseThrottle = pulseMin;
  pulseRoll = pulseMin;

  pulseAux1 = pulseMin;
  
  // This is used to count the cycles btween button presses
  // TODO: There is a lib for this,...
  tick = 0;
  tickButton = 0;

  aux1State = 0;
  
  // Set all the Piins
  pinMode( pinPPM, OUTPUT );
  pinMode( pinBuzzer, OUTPUT );
  pinMode( pinTrimRollU, INPUT );
  pinMode( pinTrimRollD, INPUT );
  pinMode( pinTrimPitchU, INPUT );
  pinMode( pinTrimPitchD, INPUT );
  pinMode( pinTrimYawU, INPUT );
  pinMode( pinTrimYawD, INPUT );
  pinMode( pinAux1, INPUT );
  
  // All our Inputs are low active, so we internally pull them all high
  digitalWrite( pinTrimRollU, HIGH );
  digitalWrite( pinTrimRollD, HIGH );
  digitalWrite( pinTrimPitchU, HIGH );
  digitalWrite( pinTrimPitchD, HIGH );
  digitalWrite( pinTrimYawU, HIGH );
  digitalWrite( pinTrimYawD, HIGH );
  digitalWrite( pinAux1, HIGH );

  // Load the trimmer settings from EEPROM
  trimYaw = trimYawSave = EEPROM.read(1);
  trimPitch = trimPitchSave = EEPROM.read(2);
  trimRoll = trimRollSave = EEPROM.read(3);  

  // Setup timer
  TCCR1A = B00110001; // Compare register B used in mode '3'
  TCCR1B = B00010010; // WGM13 and CS11 set to 1
  TCCR1C = B00000000; // All set to 0
  TIMSK1 = B00000010; // Interrupt on compare B
  OCR1A = 22500; // 22,5mS PPM output refresh
 
  // When we completed the setup we *beep-beep-beep* to signalize that we are ready to use
  beep( 3 );
}

// Our main loop: reading an checking the values
void loop() {
  readanainputsmap();
  checklimits();
}

// Read Inputs
void readanainputsmap() {
  // Read all necessary analog ports
  rawThrottle = analogRead( pinThrottle );
  rawRoll = analogRead( pinRoll );
  rawPitch = analogRead( pinPitch );
  rawYaw = analogRead( pinYaw );

  // Since we are mapping correctly here we do not need any checks
  // TODO: Reverse channels via confug
  roll = map( rawRoll, 0, 1023, 0, pulseDiff);
  pitch = map( rawPitch, 0, 1023, pulseDiff, 0);
  yaw = map( rawYaw, 0, 1023, 0, pulseDiff);
  throttle = map( rawThrottle, 0, 1023, pulseDiff, 0 );  
  
  // Map analog inputs to PPM rates for each of the channels
  pulseThrottle = throttle + pulseMin;
  pulseYaw = yaw + pulseMin - 127 + trimYaw;
  pulsePitch = pitch + pulseMin - 127 + trimPitch;
  pulseRoll = roll + pulseMin - 127 + trimRoll;
 
  // Check if Trims have changed and are valid  
  if( ++tick >= trimUpdate ){
    if( digitalRead( pinTrimYawD ) == LOW  && trimYaw > minTrim ) trimYaw--;
    if( digitalRead( pinTrimYawU ) == LOW && trimYaw < maxTrim ) trimYaw++;
    if( digitalRead( pinTrimPitchD ) == LOW && trimPitch > minTrim ) trimPitch--;
    if( digitalRead( pinTrimPitchU ) == LOW && trimPitch <maxTrim ) trimPitch++;
    if( digitalRead( pinTrimRollD ) == LOW && trimRoll > minTrim ) trimRoll--;
    if( digitalRead( pinTrimRollU ) == LOW && trimRoll < maxTrim ) trimRoll++;
    
    tick = 0;
  }
  
  if( ++tickButton >= buttonUpdate ){
    if( digitalRead( pinAux1 ) == LOW ){
      aux1State = ++aux1State % 3;
      switch( aux1State ){
        case 0: pulseAux1 = 700;  break;
        case 1: pulseAux1 = 1200; break;
        case 2: pulseAux1 = 1700; break;
        default: pulseAux1 = 700;
      }  
      beep( aux1State + 1 );
    }
    tickButton = 0;
  }

  // Saves trimmer values to EEPROM in case they changed
  if( trimYaw != trimYawSave || trimPitch != trimPitchSave || trimRoll != trimRollSave ){
    digitalWrite( pinBuzzer, HIGH );
    
    if( trimYaw != trimYawSave ) EEPROM.write( 1, trimYawSave = trimYaw );
    if( trimPitch != trimPitchSave ) EEPROM.write( 2, trimPitchSave = trimPitch );
    if( trimRoll != trimRollSave ) EEPROM.write( 3, trimRollSave = trimRoll );
 
    delay( 50 );
    digitalWrite( pinBuzzer, LOW );
  }
}

// Beep n times
void beep( int n ){
  for( int i = 0; i < n; i++ ){
    digitalWrite( pinBuzzer, HIGH );
    delay( 60 );
    digitalWrite( pinBuzzer, LOW );  
    delay( 50 );
  }
}
